#ifndef GRAPHALGORITHMS_H
#define GRAPHALGORITHMS_H
#pragma warning (disable:4786)

//------------------------------------------------------------------------
//
//  Name:   GraphSearches.h
//
//  Desc:   classes to implement graph algorithms, including DFS, BFS,
//          Dijkstra's, A*, Prims etc. (based upon the code created
//          by Robert Sedgewick in his book "Algorithms in C++")
//
//          Any graphs passed to these functions must conform to the
//          same interface used by the SparseGraph
//          
//
//------------------------------------------------------------------------
#include <vector>
#include <list>
#include <queue>
#include <stack>

#include "../graph/SparseGraph.h"
#include "../misc/PriorityQueue.h"


//----------------------------- Graph_SearchDFS -------------------------------
//
//  class to implement a depth first search. 
//-----------------------------------------------------------------------------
template<class graph_type>
class Graph_SearchDFS
{
private:
    //to aid legibility
    enum {visited, unvisited, no_parent_assigned};

    //create a typedef for the edge and node types used by the graph
    typedef typename graph_type::EdgeType Edge;
    typedef typename graph_type::NodeType Node;

    //a reference to the graph to be searched
    const graph_type& m_Graph;

    //this records the indexes of all the nodes that are visited as the
    //search progresses
    std::vector<int> m_Visited;

    //this holds the route taken to the target. Given a node index, the value
    //at that index is the node's parent. ie if the path to the target is
    //3-8-27, then m_Route[8] will hold 3 and m_Route[27] will hold 8.
    std::vector<int>  m_Route;

    //As the search progresses, this will hold all the edges the algorithm has
    //examined. THIS IS NOT NECESSARY FOR THE SEARCH, IT IS HERE PURELY
    //TO PROVIDE THE USER WITH SOME VISUAL FEEDBACK
    std::vector<const Edge*> m_SpanningTree;

    //the source and target node indices
    int m_iSource;
    int m_iTarget;

    //true if a path to the target has been found
    bool m_bFound;


    //this method performs the DFS search
    bool search();
  
public:
    Graph_SearchDFS(const graph_type& graph, int source, int target = -1 ):
                                                  m_Graph(graph),
                                                  m_iSource(source),
                                                  m_iTarget(target),
                                                  m_bFound(false),
                                                  m_Visited(m_Graph.getNumNodes(), unvisited),
                                                  m_Route(m_Graph.getNumNodes(), no_parent_assigned)

    {                                                                         
        m_bFound = search(); 
    }


    //returns a vector containing pointers to all the edges the search has examined
    std::vector<const Edge*> getSearchTree()const{return m_SpanningTree;}

    //returns true if the target node has been located
    bool isFound()const{return m_bFound;}

  //returns a vector of node indexes that comprise the shortest path
  //from the source to the target
  std::list<int> getPathToTarget() const;  

};

//-----------------------------------------------------------------------------
template <class graph_type>
bool Graph_SearchDFS<graph_type>::search()
{
    //create a std stack of edges
    std::stack<const Edge*> stack;

    //create a dummy edge and put on the stack
    Edge Dummy(m_iSource, m_iSource, 0);

    stack.push(&Dummy);

    //while there are edges in the stack keep searching
    while (!stack.empty())
    {
        //grab the next edge
        const Edge* Next = stack.top();

        //remove the edge from the stack
        stack.pop();

        //make a note of the parent of the node this edge points to
        m_Route[Next->to()] = Next->from();

        //put it on the tree. (making sure the dummy edge is not placed on the tree)
        if (Next != &Dummy)
        {
            m_SpanningTree.push_back(Next);
        }

        //and mark it visited
        m_Visited[Next->to()] = visited;

        //if the target has been found the method can return success
        if (Next->to() == m_iTarget)
        {
            return true;
        }

        //push the edges leading from the node this edge points to onto
        //the stack (provided the edge does not point to a previously 
        //visited node)
        graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, Next->to());

        for (const Edge* pE=ConstEdgeItr.begin(); !ConstEdgeItr.end(); pE=ConstEdgeItr.next())
        {
            if (m_Visited[pE->to()] == unvisited)
            {
                stack.push(pE);
            }
        }
    }

    //no path to target
    return false;
}

//-----------------------------------------------------------------------------
template <class graph_type>
std::list<int> Graph_SearchDFS<graph_type>::getPathToTarget()const 
{
    std::list<int> path;

    //just return an empty path if no path to target found or if
    //no target has been specified
    if (!m_bFound || m_iTarget<0) return path;

    int nd = m_iTarget;

    path.push_front(nd);

    while (nd != m_iSource)
    {
        nd = m_Route[nd];

        path.push_front(nd);
    }

    return path;
}



//----------------------------- Graph_SearchBFS -------------------------------
//
//-----------------------------------------------------------------------------
template<class graph_type>
class Graph_SearchBFS
{
private:
    //to aid legibility
    enum {visited, unvisited, no_parent_assigned};

    //create a typedef for the edge type used by the graph
    typedef typename graph_type::EdgeType Edge;

    //a reference to the graph to be searched
    const graph_type& m_Graph;

    //this records the indexes of all the nodes that are visited as the
    //search progresses
    std::vector<int> m_Visited;

    //this holds the route taken to the target. Given a node index, the value
    //at that index is the node's parent. ie if the path to the target is
    //3-8-27, then m_Route[8] will hold 3 and m_Route[27] will hold 8.
    std::vector<int> m_Route;

    //the source and target node indices
    int m_iSource;
    int m_iTarget;

    //true if a path to the target has been found
    bool m_bFound;

    //As the search progresses, this will hold all the edges the algorithm has
    //examined. THIS IS NOT NECESSARY FOR THE SEARCH, IT IS HERE PURELY
    //TO PROVIDE THE USER WITH SOME VISUAL FEEDBACK
    std::vector<const Edge*> m_SpanningTree;


    //the BFS algorithm is very similar to the DFS except that it uses a
    //FIFO queue instead of a stack.
    bool search();

    
public:
    Graph_SearchBFS(const graph_type& graph, int source, int target = -1 ):m_Graph(graph),
                                                m_iSource(source),
                                                m_iTarget(target),
                                                m_bFound(false),
                                                m_Visited(m_Graph.getNumNodes(), unvisited),
                                                m_Route(m_Graph.getNumNodes(), no_parent_assigned)

    {                                                                         
        m_bFound = search();   
    }

    bool isFound()const{return m_bFound;}

    //returns a vector containing pointers to all the edges the search has examined
    std::vector<const Edge*> getSearchTree()const{return m_SpanningTree;}

    //returns a vector of node indexes that comprise the shortest path
    //from the source to the target
    std::list<int> getPathToTarget()const; 

};

//-----------------------------------------------------------------------------

template <class graph_type>
bool Graph_SearchBFS<graph_type>::search()
{
    //create a std queue of edges
    std::queue<const Edge*> Q;

    const Edge Dummy(m_iSource, m_iSource, 0);

    //create a dummy edge and put on the queue
    Q.push(&Dummy);

    //mark the source node as visited
    m_Visited[m_iSource] = visited;

    //while there are edges in the queue keep searching
    while (!Q.empty())
    {
        //grab the next edge
        const Edge* Next = Q.front();

        Q.pop();

        //mark the parent of this node
        m_Route[Next->to()] = Next->from();

        //put it on the tree. (making sure the dummy edge is not placed on the tree)
        if (Next != &Dummy)
        {
            m_SpanningTree.push_back(Next);
        }

        //exit if the target has been found
        if (Next->to() == m_iTarget)
        {
            return true;
        }

        //push the edges leading from the node at the end of this edge 
        //onto the queue
        graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, Next->to());

        for (const Edge* pE=ConstEdgeItr.begin();!ConstEdgeItr.end(); pE=ConstEdgeItr.next())
        {
            //if the node hasn't already been visited we can push the
            //edge onto the queue
            if (m_Visited[pE->to()] == unvisited)
            {
                Q.push(pE);

                //and mark it visited
                m_Visited[pE->to()] = visited;
            }
        }
    }

    //no path to target
    return false;
}


//-----------------------------------------------------------------------------
template <class graph_type>
std::list<int> Graph_SearchBFS<graph_type>::getPathToTarget()const 
{
    std::list<int> path;

    //just return an empty path if no path to target found or if
    //no target has been specified
    if (!m_bFound || m_iTarget<0) return path;

    int nd = m_iTarget;

    path.push_front(nd);

    while (nd != m_iSource)
    {
        nd = m_Route[nd];

        path.push_front(nd);
    }

    return path;
}



//----------------------- Graph_SearchDijkstra --------------------------------
//
//  Given a graph, source and optional target this class solves for
//  single source shortest paths (without a target being specified) or 
//  shortest path from source to target.
//
//  The algorithm used is a priority queue implementation of Dijkstra's.
//  note how similar this is to the algorithm used in Graph_MinSpanningTree.
//  The main difference is in the calculation of the priority in the line:
//  
//  float NewCost = m_CostToThisNode[best] + pE->Cost;
//------------------------------------------------------------------------
template <class graph_type>
class Graph_SearchDijkstra
{
private:
    //create a typedef for the edge type used by the graph
    typedef typename graph_type::EdgeType Edge;
  
    const graph_type& m_Graph;

    //this vector contains the edges that comprise the shortest path tree -
    //a directed subtree of the graph that encapsulates the best paths from 
    //every node on the SPT to the source node.
    std::vector<const Edge*> m_ShortestPathTree;

    //this is indexed into by node index and holds the total cost of the best
    //path found so far to the given node. For example, m_CostToThisNode[5]
    //will hold the total cost of all the edges that comprise the best path
    //to node 5, found so far in the search (if node 5 is present and has 
    //been visited)
    std::vector<float> m_CostToThisNode; 

    //this is an indexed (by node) vector of 'parent' edges leading to nodes 
    //connected to the SPT but that have not been added to the SPT yet. This is
    //a little like the stack or queue used in BST and DST searches.
    std::vector<const Edge*> m_SearchFrontier;

    int m_iSource;
    int m_iTarget;

    void search();  
    
public:
    Graph_SearchDijkstra(const graph_type& graph, int source, int target = -1):m_Graph(graph),
                                                               m_ShortestPathTree(graph.getNumNodes()),                              
                                                               m_SearchFrontier(graph.getNumNodes()),
                                                               m_CostToThisNode(graph.getNumNodes()),
                                                               m_iSource(source),
                                                               m_iTarget(target)
    {                                           
        search();     
    }
 
    //returns the vector of edges that defines the SPT. If a target was given
    //in the constructor then this will be an SPT comprising of all the nodes
    //examined before the target was found, else it will contain all the nodes
    //in the graph.
    std::vector<const Edge*> getSPT()const{return m_ShortestPathTree;}

    //returns a vector of node indexes that comprise the shortest path
    //from the source to the target. It calculates the path by working
    //backwards through the SPT from the target node.
    std::list<int> getPathToTarget()const;

    //returns the total cost to the target
    float getCostToTarget()const{return m_CostToThisNode[m_iTarget];}

    //returns the total cost to the given node
    float getCostToNode(unsigned int nd)const{return m_CostToThisNode[nd];}

};


//-----------------------------------------------------------------------------
template <class graph_type>
void Graph_SearchDijkstra<graph_type>::search()
{
    //create an indexed priority queue that sorts smallest to largest
    //(front to back).Note that the maximum number of elements the iPQ
    //may contain is N. This is because no node can be represented on the 
    //queue more than once.
    indexedPriorityQLow<float> pq(m_CostToThisNode, m_Graph.getNumNodes());

    //put the source node on the queue
    pq.insert(m_iSource);

    //while the queue is not empty
    while(!pq.empty())
    {
        //get lowest cost node from the queue. Don't forget, the return value
        //is a *node index*, not the node itself. This node is the node not already
        //on the SPT that is the closest to the source node
        int NextClosestNode = pq.pop();

        //move this edge from the frontier to the shortest path tree
        m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode];

        //if the target has been found exit
        if (NextClosestNode == m_iTarget) return;

        //now to relax the edges.
        graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, NextClosestNode);

        //for each edge connected to the next closest node
        for (const Edge* pE=ConstEdgeItr.begin(); !ConstEdgeItr.end(); pE=ConstEdgeItr.next())
        {
            //the total cost to the node this edge points to is the cost to the
            //current node plus the cost of the edge connecting them.
            float NewCost = m_CostToThisNode[NextClosestNode] + pE->getCost();

            //if this edge has never been on the frontier make a note of the cost
            //to get to the node it points to, then add the edge to the frontier
            //and the destination node to the PQ.
            if (m_SearchFrontier[pE->to()] == 0)
            {
                m_CostToThisNode[pE->to()] = NewCost;

                pq.insert(pE->to());

                m_SearchFrontier[pE->to()] = pE;
            }

            //else test to see if the cost to reach the destination node via the
            //current node is cheaper than the cheapest cost found so far. If
            //this path is cheaper, we assign the new cost to the destination
            //node, update its entry in the PQ to reflect the change and add the
            //edge to the frontier
            else if ( (NewCost < m_CostToThisNode[pE->to()]) && (m_ShortestPathTree[pE->to()] == 0) )
            {
                m_CostToThisNode[pE->to()] = NewCost;

                //because the cost is less than it was previously, the PQ must be
                //re-sorted to account for this.
                pq.ChangePriority(pE->to());

                m_SearchFrontier[pE->to()] = pE;
            }
        }
    }
}

//-----------------------------------------------------------------------------
template <class graph_type>
std::list<int> Graph_SearchDijkstra<graph_type>::getPathToTarget()const
{
    std::list<int> path;

    //just return an empty path if no target or no path found
    if (m_iTarget < 0)  return path;

    int nd = m_iTarget;

    path.push_front(nd);

    while ((nd != m_iSource) && (m_ShortestPathTree[nd] != 0))
    {
        nd = m_ShortestPathTree[nd]->from();

        path.push_front(nd);
    }

    return path;
}

//------------------------------- Graph_SearchAStar --------------------------
//
//  this searchs a graph using the distance between the target node and the 
//  currently considered node as a heuristic.
//
//  This search is more commonly known as A* (pronounced Ay-Star)
//-----------------------------------------------------------------------------
template <class graph_type, class heuristic>
class Graph_SearchAStar
{
private:
    //create a typedef for the edge type used by the graph
    typedef typename graph_type::EdgeType Edge;


    const graph_type& m_Graph;

    //indexed into my node. Contains the 'real' accumulative cost to that node
    std::vector<float> m_GCosts; 

    //indexed into by node. Contains the cost from adding m_GCosts[n] to
    //the heuristic cost from n to the target node. This is the vector the
    //iPQ indexes into.
    std::vector<float> m_FCosts;

    std::vector<const Edge*> m_ShortestPathTree;
    std::vector<const Edge*> m_SearchFrontier;

    int m_iSource;
    int m_iTarget;

    //the A* search algorithm
    void search();
    
public:
    Graph_SearchAStar(graph_type &graph, int source, int target):m_Graph(graph),
                                              m_ShortestPathTree(graph.getNumNodes()),                              
                                              m_SearchFrontier(graph.getNumNodes()),
                                              m_GCosts(graph.getNumNodes(), 0.0),
                                              m_FCosts(graph.getNumNodes(), 0.0),
                                              m_iSource(source),
                                              m_iTarget(target)
    {
        search();   
    }
 
    //returns the vector of edges that the algorithm has examined
    std::vector<const Edge*> getSPT()const{return m_ShortestPathTree;}

    //returns a vector of node indexes that comprise the shortest path
    //from the source to the target
    std::list<int> getPathToTarget()const;

    //returns the total cost to the target
    float getCostToTarget()const{return m_GCosts[m_iTarget];}

};

//-----------------------------------------------------------------------------
template <class graph_type, class heuristic>
void Graph_SearchAStar<graph_type, heuristic>::search()
{
    //create an indexed priority queue of nodes. The nodes with the
    //lowest overall F cost (G+H) are positioned at the front.
    indexedPriorityQLow<float> pq(m_FCosts, m_Graph.getNumNodes());

    //put the source node on the queue
    pq.insert(m_iSource);

    //while the queue is not empty
    while(!pq.empty())
    {
        //get lowest cost node from the queue
        int NextClosestNode = pq.pop();

        //move this node from the frontier to the spanning tree
        m_ShortestPathTree[NextClosestNode] = m_SearchFrontier[NextClosestNode];

        //if the target has been found exit
        if (NextClosestNode == m_iTarget) return;

        //now to test all the edges attached to this node
        graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, NextClosestNode);

        for (const Edge* pE=ConstEdgeItr.begin();
        !ConstEdgeItr.end(); 
        pE=ConstEdgeItr.next())
        {
            //calculate the heuristic cost from this node to the target (H)                       
            float HCost = heuristic::Calculate(m_Graph, m_iTarget, pE->to()); 

            //calculate the 'real' cost to this node from the source (G)
            float GCost = m_GCosts[NextClosestNode] + pE->getCost();

            //if the node has not been added to the frontier, add it and update
            //the G and F costs
            if (m_SearchFrontier[pE->to()] == NULL)
            {
                m_FCosts[pE->to()] = GCost + HCost;
                m_GCosts[pE->to()] = GCost;

                pq.insert(pE->to());

                m_SearchFrontier[pE->to()] = pE;
            }

            //if this node is already on the frontier but the cost to get here
            //is cheaper than has been found previously, update the node
            //costs and frontier accordingly.
            else if ((GCost < m_GCosts[pE->to()]) && (m_ShortestPathTree[pE->to()]==NULL))
            {
                m_FCosts[pE->to()] = GCost + HCost;
                m_GCosts[pE->to()] = GCost;

                pq.ChangePriority(pE->to());

                m_SearchFrontier[pE->to()] = pE;
            }
        }
    }
}

//-----------------------------------------------------------------------------
template <class graph_type, class heuristic>
std::list<int> Graph_SearchAStar<graph_type, heuristic>::getPathToTarget()const
{
    std::list<int> path;

    //just return an empty path if no target or no path found
    if (m_iTarget < 0)  return path;    

    int nd = m_iTarget;

    path.push_front(nd);
    
    while ((nd != m_iSource) && (m_ShortestPathTree[nd] != 0))
    {
        nd = m_ShortestPathTree[nd]->from();

        path.push_front(nd);
    }

    return path;
} 



//---------------------- Graph_MinSpanningTree --------------------------------
//
//  given a graph and a source node you can use this class to calculate
//  the minimum spanning tree. If no source node is specified then the 
//  algorithm will calculate a spanning forest starting from node 1 
//
//  It uses a priority first queue implementation of Prims algorithm
//------------------------------------------------------------------------
template <class graph_type>
class Graph_MinSpanningTree
{
private:
    //create a typedef for the edge type used by the graph
    typedef typename graph_type::EdgeType Edge;

    const graph_type& m_Graph;

    std::vector<float> m_CostToThisNode; 

    std::vector<const Edge*> m_SpanningTree;
    std::vector<const Edge*> m_Fringe;

    void search(const int source)
    {
        //create a priority queue
        indexedPriorityQLow<float> pq(m_CostToThisNode, m_Graph.getNumNodes());

        //put the source node on the queue
        pq.insert(source);

        //while the queue is not empty
        while(!pq.empty())
        {
            //get lowest cost edge from the queue
            int best = pq.pop();

            //move this edge from the fringe to the spanning tree
            m_SpanningTree[best] = m_Fringe[best];

            //now to test the edges attached to this node
            graph_type::ConstEdgeIterator ConstEdgeItr(m_Graph, best);

            for (const Edge* pE=ConstEdgeItr.beg(); !ConstEdgeItr.end(); pE=ConstEdgeItr.nxt())
            {
                float Priority = pE->cost;

                if (m_Fringe[pE->to()] == 0)
                {
                    m_CostToThisNode[pE->to()] = Priority;

                    pq.insert(pE->to());

                    m_Fringe[pE->to()] = pE;
                }

                else if ((Priority<m_CostToThisNode[pE->to()]) && (m_SpanningTree[pE->to()]==0))
                {
                    m_CostToThisNode[pE->to()] = Priority;

                    pq.ChangePriority(pE->to());

                    m_Fringe[pE->to()] = pE;
                }
            }
        }
    }

public:

    Graph_MinSpanningTree(graph_type &G,int source = -1):m_Graph(G),
                                                       m_SpanningTree(G.getNumNodes()),  
                                                       m_Fringe(G.getNumNodes()),
                                                       m_CostToThisNode(G.getNumNodes(), -1)
    {                                                                             
        if (source < 0)
        {
            for (int nd=0; nd<G.getNumNodes(); ++nd)
            {
                if (m_SpanningTree[nd] == 0)
                {
                    search(nd);
                }
            }
        }

        else
        {
            search(source);   
        }
    }

    std::vector<const Edge*> getSpanningTree()const{return m_SpanningTree;}


    
};

#endif
